/*
 * Arduino based test of the fixmath library
 */

#include <WProgram.h>
#include <stdio.h>
#include <avr/io.h>
#include <avr/pgmspace.h>
#include "test.h"
#include "fixed.h"

#define CHECK_EQ(expected, actual) CHECK((expected) == (actual))
#define CHECK_NR(expected, actual) CHECK(nearly_equal(expected, actual))

// Tests that FC and basic initialization works as expected
DEF_TEST(FC)
	{
		CHECK(FC(0.9) + F0 > FC(0.8));
		Fixed f03 = FC(0.3);
		CHECK(FC(0.9) + f03 < F0);
		CHECK_EQ(FC(0), F0);
		CHECK_EQ (FC(0.5), 0x4000);
		CHECK_EQ(FC(-0.5), (~0x4000) + 1);
		CHECK_EQ(FC(-0.5), -0x4000);
		CHECK_EQ(FC(-0.5), (int16_t) 0xC000);
	}
	END_TEST

DEF_TEST(Multiply)
	{
		CHECK_EQ(fix_mul(FC(0.5), FC(0.5)), FC(0.25));
		CHECK_EQ(fix_mul(0x8000, 0x4000), (Fixed) 0xC000); // -1 * 0.5 = -0.5
		CHECK_EQ(fix_mul(FC(-1), FC(0.5)), FC(-0.50));

		CHECK_EQ(fix_mul(0xff, 0xff), 0x0001); // Check lo * lo carries
		CHECK_NR(fix_mul(FC(-0.23), FC(0.90)), FC(-0.207));
		// Tests here
	}
	END_TEST

DEF_TEST(Trig)
	{
		// sin
		CHECK_EQ(fix_sin( 0x1000, 0x2000), FC(0.894427));
		CHECK_EQ(fix_sin(-0x1000, 0x2000), FC(0.894427));
		CHECK_EQ(fix_sin( 0x1000, -0x2000), -FC(0.894427));
		CHECK_EQ(fix_sin(-0x1000, -0x2000), -FC(0.894427));

		// cos
		CHECK_EQ(fix_cos( 0x1000, 0x2000), FC(0.447214));
		CHECK_EQ(fix_cos( -0x1000, 0x2000), -FC(0.447214));
		CHECK_EQ(fix_cos( 0x1000, -0x2000), FC(0.447214));
		CHECK_EQ(fix_cos( -0x1000, -0x2000), -FC(0.447214));

		// shifting up
		CHECK_EQ(fix_sin( 0x0001, 0x0002), FC(0.894427));
		CHECK_EQ(fix_sin( 0x0800, 0x1000), FC(0.894427));

		// sin2 + cos2 == 1 (ish)
		for (int16_t adj = 0; adj < 16; adj++) {
			for (int16_t opp = 0; opp < 16; opp++) {
				if (adj != 0 || opp != 0) {
					Fixed s = fix_sin(adj, opp);
					Fixed c = fix_cos(adj, opp);
					Fixed one = fix_mul(s, s) + fix_mul(c, c);
					CHECK(one > FC(0.999));
				}
			}
		}
	}
	END_TEST

void run_tests(void) {
	PRINT_LITERAL("\nTests\n=====\n");
	int ok = 1;
	ok = ok && CALL_TEST(FC);
	ok = ok && CALL_TEST(Multiply);
	ok = ok && CALL_TEST(Trig);
	if (ok) {
		PRINT_LITERAL("PASS\n");
	} else {
		PRINT_LITERAL("Failure detected\n");
	}
}
